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[57] ABSTRACT 

The present invention discloses systematic methods and 
apparatus for the design of real time controllers. Real- 
time control employs adaptive force/position by use of 
feedforward and feedback controllers, with the feedfor- 
ward controller being the inverse of the linearized 
model of robot dynamics and containing only propor- 
tional-double-derivative terms is disclosed. The feed- 
back controller, of the proportional-integral-derivative 
type, ensures that manipulator joints follow reference 
trajectories and the feedback controller achieves robust 
tracking of step-plus-exponential trajectories, all in real 
time. 

The adaptive controller includes adaptive force and 
position control within a hybrid control architecture. 
The adaptive controller, for force control, achieves 
tracking of desired force setpoints, and the adaptive 
position controller accomplishes tracking of desired 
position trajectories. Circuits in the adaptive feedback 
and feedforward controllers are varied by adaptation 
laws. 

28 Claims, 11 Drawing Sheets 
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FIG. 8o 
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FORCE Px(t) UNDER 
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FIG. 9a 

RESPONSE OF CONTACT FORCE P x (t) 

IN CASE 1 



FIG. 9b 


RESPONSE OF POSITION y(t) IN CASE 1 
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FIG. 10 

RESPONSE OF FORCE P x (t) TO SUDDEN STIFFNESS 
GHANGE IN CASE 2 



FIG. I la 

RESPONSE OF FORCE P x (t)TO SURFACE 
MOVEMENT IN CASE 3 
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FIG. I 2a 


RESPONSE OF FORCE P x (t) IN COMPLIANT 
MOTION IN CASE 4 



FIG. 12b 

POSITION OF REACTION SURFACE x 0 ( + ) IN CASE 4 
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METHOD AND APPARATUS FOR ADAPTIVE 
FORCE AND POSITION CONTROL OF 
MANIPULATORS 

5 

ORIGIN OF THE INVENTION 

The invention described herein was made in the per- 
formance of work under a NASA Contract and is sub- 
ject to the provisions of Public Law 96517 (35 USC 202) 
in which the contractor has elected to retain title. 10 

BACKGROUND OF THE INVENTION 

1. Field of the Invention 

The field of this invention relates to control systems 
for robotic manipulators and more particularly relates 15 
to adaptive force and position controllers combined in a 
hydrid control system. 

2. Background of the Invention 

Hybrid control systems, i.e., those systems which 
together supply signals to control the position of, and 20 
force exerted by, a robotic manipulator simultaneously 
are known. Such systems are required because most 
manipulators are operated in the presence of environ- 
mental constraints, and certain degrees-of-freedom are 
lost for motion due to the environmental constraints. 25 
For example, when the manipulator makes contact with 
the environment, the contact forces must be controlled 
in the constraint directions, while the positions are con- 
trolled simultaneously in the free directions. 

The problem of manipulator control in a constrained 30 
environment has been investigated by several research- 
ers. See, for example D. E. Whitney, “Historical Per- 
spective and State of the Art in Robot Force Control,” 
Proc. IEEE Intern. Conf. on Robotics and Automation, 
pp. 262-268, St. Louis, MO, 1985. At present, three 35 
major conceptual approaches exist for simultaneous 
position and force control. Paul and Shimano (R. P. 
Paul and B. Shimano, “Compliance and Control,” Proc. 
Joint Aut. Control Conf., pp. 694-699, San Francisco, 
CA, 1976) suggest a method which uses certain joints 40 
for position control while the remaining joints are used 
for force control. Salisbury (J. K. Salisbury, “Active 
Stiffness Control of a Manipulator in Cartesian Coordi- 
nates,” Proc. IEEE Conf. on Decision and Control, pp. 
95-100, Albuquerque, NM, 1980) puts forward a tech- 45 
nique for controlling the end-effector stiffness charac- 
teristics in the Cartesian space. Raibert and Craig (M. 

H. Raibert and J. J. Craig, “Hybrid Position/Force 
Control of Manipulators,” ASME J. Dyn Systems, Mea- 
surement and Control, Vol. 102, pp. 126-133, 1981) pro- 
posed a conceptual architecture, based on the analysis 
of Mason for “hydrid control” (M. T. Mason, “Compli- 
ance and Force Control for Computer Controlled Ma- 
nipulators,” IEEE Trans. Systems, Man and Cybernetics, 
SMC\ 1(6), pp. 418-432, 1981). Hybrid control allows 
forces to be controlled in the constraint directions by a 
force controller, while simultaneously controlling posi- 
tions in the free direction by a position controller. Raib- 
ert and Craig, however, do not prescribe a general and 
systematic method for the design of position and force 
controllers. Nevertheless, hybrid control has gained 
considerable popularity over the other two alternatives 
for simultaneous position and force control. Hybrid 
control has been used extensively in the literature. See, 
for example, H. Zhang and R. P. Paul, “Hybrid Control 
of Robot Manipulators,” Proc. IEEE Intern. Conf. on 
Robotics and Automation, pp. 602-607, St. Louis, MO, 
1985; H. West and H. Asada, “A Method for the Design 
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of Hybrid Position/Force Controllers for Manipulators 
Constrained by Contact with the Environment,” Proc. 
IEEE Intern. Conf. on Robotics and Automation, pp. 
251-259, St. Louis, MO, 1985; P. G. Backes, G. G. 
Leininger and C. H. Chung, “Real-Time Cartesian Co- 
ordinate Hybrid Control of a PUMA 560 Manipulator,” 
Proc. IEEE Intern. Conf. on Robotics and Automation, 
pp. 608-613, St. Louis, MO, 1985; P. G. Backes, G. G. 
Leininger and C. H. Chung, “Joint Self-Tuning with 
Cartesian Setpoints,” ASME J. Dyn. Systems, Measure- 
ment and Control, Vol 108, pp. 146-150, 1986; O. Khatib 
and J. Burdick, “Motion and Force Control of Robot 
Manipulators,” Proc. IEEE Intern. Conf. on Robotics 
and Automation, pp. 1381-1386, San Francisco, CA, 
1986; T. Yoshikawa, “Dynamic Hybrid Position/Force 
Control of Robot Manipulators,” Proc. IEEE Intern. 
Conf. on Robotics and Automation, pp. 1393-1398, San 
Francisco, CA, 1986; A. J. Koivo, “Force-Position- 
Velocity Control with Self-Tuning for Robotic Manip- 
ulators,” Proc. IEEE Intern. Conf. on Robotics and Auto- 
mation, pp. 1563-1568, San Francisco, CA, 1986; C. C. 
Nguyen, F. J. Pooran a.nd T. Premack, “Control of 
Robot Manipulator Compliance,” Proc. Intern. Symp. on 
Robot Manipulators, Albuquerque, NM, 1986; O. Kha- 
tib, “Dynamic Control of Manipulators in Operational 
Space,” Proc. 6th IFToMM Congress on Theory of 
Machines and Mechanisms, pp. 1128-1131, New Delhi, 
India, 1983 (Whiley, New Delhi); S. D. Eppinger and 
W. P. Seering, “On Dynamic Models of Robot Force 
Control,” Proc. IEEE Intern. Conf. on Robotics and 
Automation, pp. 29-34, San Francisco, CA 1986; and J. 
W. Gilbart, R. V. Monopoli and C. F. Price, “Improved 
Convergence and Increased Flexibility in the Design of 
Model Reference Adaptive Control Systems,” Proc. 
IEEE Conf. on Decision and Control, pp. IV 3.1-3.10, 
Austin, TX, 1970. 

Position sensors and force sensors are well known 
and actually measure the position and force at a con- 
trolled end effector. The end effector behavior, 
whether position alone, or position and force, results 
from voltages applied to motors that run the joints in a 
robotic arm. In Raibert and Craig, the hybrid control 
system requires coordinate transformations of the mea- 
surements to develop a feedback term, which through 
very extensive inverse Jacobian computations, controls 
the motors in the joints of the robotic arm by indepen- 
dent servo control loops. Such an approach requires 
knowledge of those motors parameters and other ro- 
botic arm parameter values, as well. Furthermore, inde- 
pendent servo control loops, as suggested by Raibert 
and Craig, do not accommodate cross coupling of posi- 
tion and force. 

This invention, in distinction from Raibert and Craig, 
is a truly adaptive controller for force and position. 
Since the adaptive controller senses force and position 
in Cartesian space and formulates driving voltages 
which achieve the Cartesian force and position, ad- 
vance knowledge of the robotic arm, joint motor pa- 
rameter and environment values is not necessary. A 
highly stable and adaptive real-time controller is 
achieved by this invention. 

SUMMARY OF THE INVENTION 

The present invention discloses systematic methods 
and apparatus for the design of adaptive force and posi- 
tion controllers within a hybrid control architecture. 
The adaptive force controller achieves tracking of de- 



4 , 860,215 


3 

sired force setpoints, and the adaptive position control- 
ler accomplishes tracking of desired position trajecto- • 
ries. Force and position controller gains are varied by 
adaptation laws and the controllers are computationally 
fast and suitable for on-line implementation with high 5 
sampling rates. The novel adaptive controller of this 
invention comprises adaptive feedback and feedforward 
controllers. These controllers are capable of compen- 
sating for dynamic cross-couplings that exist between 
position and force control loops. The adaptive control- 10 
lers do not require knowledge of the complex dynamic 
model or parameter values of the manipulator or the 
environment. The force and position controllers are 
linear and stable and generate real-time signals which 
vary and compensate for system non-linearities in order 15 
to achieve a desired position/force response. 

There are certain key differences between the inven- 
tion’s approach and the conventional hybrid control 
approach of Raibert and Craig. Firstly, in the present 
invention, the force or position control problems are 20 
formulated in the Cartesian space with the end-effector 
Cartesian forces as the manipulated variables; whereas 
in Raibert and Craig, the problems are formulated in the 
joint space. The invention’s formulation results in com- 
putational improvement since inverse Jacobians are not 25 
needed in the controllers. Secondly, the invention oper- 
ates on the measured variables so as to produce the 
position and force variables that need to be controlled; 
whereas in Raibert and Craig, a selection matrix and its 
complement are used after formation of tracking errors. 30 

An additionally attractive feature of the adaptive 
controllers of this invention is an ability to compensate 
for dynamic cross-couplings that exist between the posi- 
tion and force control loops in the hybrid control archi- 
tecture. Furthermore, the adaptive force and position 35 
controllers have “learning capabilities” to cope with 
unpredictable changes in the manipulator or environ- 
ment parameters such as environment or robotic arm 
stiffness. This is due to the fact that the controller gains 
are adapted rapidly on the basis of the manipulator’s 40 
Cartesian space performance. Low computational re- 
quirements make the control loops in accordance with 
the principles of this invention suitable for implementa- 
tion in on-line hydrid control with high sampling rates. 

In short, the present invention is more straightfor- 45 
ward and appealing than any known conventional ap- 
proach. 

BRIEF DESCRIPTION OF THE DRAWINGS 

FIG. 1 depicts a block diagram of an improved hy- 50 
brid control architecture in keeping with this invention; 

FIG. 2 is a simplified dynamic model of a robotic arm 
and environment which is useful in promoting a clearer 
understanding of certain aspects relevant to the inven- 
tion; 55 

FIG. 3 is a simplified block diagram of a controller 
for performing adaptive force control in accordance 
with the invention; 

FIG. 4 is a simplified block diagram of a controller 
for performing adaptive position control in accordance 60 
with the invention; 

FIG. 5 is a block diagram of a basic module which is 
repeated as necessary for implementation of the control- 
ler functions of adaptive force and position control in 
accordance with this invention; 65 

FIG. 6 is a two-link planar manipulator, end effector 
and reaction surface useful in understanding an illustra- 
tive example of the invention; 


4 

FIG. 7 is an embodiment of the invention which 
included simulation by a computer operation; 

FIG. 8 includes FIGS. 8a and 8 b which respectively 
reflect an evaluation of the invention for responses to 
contact force and position under adaptive control; 

FIG. 9 includes FIGS. 9 a and 9b and represents an 
evaluation of the performance of the controller of the 
invention; 

FIG. 10 includes FIGS. 10a and 106 and represent 
curves useful in demonstrating the invention’s perfor- 
mance under stiffness variation; 

FIG. 11 includes FIGS. 11a and lib which represent 
curves useful in demonstrating the invention’s perfor- 
mance when subjected to movement of a reaction sur- 
face away from an end effector; and 

FIG. 12 includes FIGS. 12a and 12 b which include 
curves useful in demonstrating the invention’s perfor- 
mance when the reaction surface moves towards and 
against an end effector; and 

FIG. 13 includes FIGS. 13a and 136 and represents 
an alternate embodiment of the invention. 

DETAILED DESCRIPTION OF THE DRAWING 

It is believed helpful to review the description of the 
drawings of FIG. 1 and FIG. 6 in conjunction with a 
review of the problem of controlling force and position 
of an end effector. 

A robot manipulator such as manipulator 600, FIG. 6, 
performs a number of different tasks in a Cartesian 
space {X}. Each task, in general, involves motion of the 
manipulator including its end-effector 605, FIG. 6, in 
certain directions; and, simultaneously, exertion of 
force by the end-effector 605 on the environment 610, 
or an object in the environment 610, in the remaining 
directions. See, for example, M. T. Mason as noted 
earlier herein. 

The directions of motion and the amount of force 
depend on the nature of the particular task to be per- 
formed and are reflected in the task descriptor 125 in 
the hybrid control system 100 shown in FIG. 1. The 
task descriptor 125 also contains the transformations 
required to map the measurements 0 and P s of joint 
encoders and force/torque sensors into position and 
force variables in the constraint frame defined with 
respect to the task geometry. See, for example, Raibert 
and Craig as noted earlier herein. Note that the desired 
force and position trajectories are also specified in the 
constraint frame. As used herein, “position” implies 
position and orientation and “force” implies force and 
torque. 

For any given task, the n-dimensional Cartesian space 
{X} can be decomposed into two orthogonal 1— and 
m-dimensional subspaces {Y} and {Z}, where n=l + m. 
The ‘‘position subspace” {Y} contains the 1 directions 
(i.e., degrees-of-freedom) in which the manipulator 
end-effector is free to move, and along which, the end- 
effector position is to be controlled. The “force sub- 
space” {Z} contains the remaining m directions in 
which the manipulator end-effector is constrained by, 
and interacts with, the environment and along which 
the contact force is to be controlled. 

In the hybrid force/position control problem solved 
by this invention, we consider the “virtual” Cartesian 
force F acting on the end-effector as the manipulated 
variable and the position or force of the end-effector as 
the controlled variables. See, for example, O. Khatib 
and J. Burdick as noted earlier herein. 
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The hydrid control architecture is based on two con- 
trollers as shown in FIG. 1; namely, the position con- 
troller 110 which operates in {Y} and the force control- 
ler 120 which acts in {Z}. The position controller 110 
generates the Cartesian end-effector force F y required 5 
to cause the end-effector motion to track a desired posi- 
tion trajectory in {Y}. The force controller 120 pro- 
duces the Cartesian end-effector force F z needed to 
ensure that the end-effector force follows a desired 
force setpoint, or command, in {Z}. Since it is not possi- 10 
ble to physically apply Cartesian forces directly to the 
end-effector, the equivalent joint torques needed to 
effectively cause these forces are computed and imple- 
mented. The required joint torques are obtained from 
the Cartesian forces by means of the Jacobian matrix, I 5 
J(0), 130 of 100, where 6 is the joint angle vector. 

In summary then, in FIG. 1, assume that it is desired 
to apply a constant force of a given amount simulta- 
neous with desired movement of an end effector. The 
commands for this operation are issued on leads 106 and 20 
107 from the position/force trajectory generator 105. 
The desired position or reference is signal R on lead 106 
into position controller 110. The other output from 
generator 105, P r , on lead 107 is the desired force signal. 

At the end effector, well-known devices measure the 25 
actual position Y and the actual force P, and signals Y 
and P are fed back over leads 108 and 109 to position 
and force controllers 110 and 120, respectively. The 
position controller 110 compares the desired position R 
and the actual position Y and derives an error, or con- 30 
trol signal, Fj, which is output to the matrix 130. The 
force controller 120 does likewise for the desired and 
actual forces, P r and P, and an error or control signal F 2 
is supplied to matrix 130. 

The adaptive controllers of this invention may be 35 
thought of as generating the components which are 
applied directly to the end effector to move it in Carte- 
sian space. Since it cannot be done directly, the matrix 
130 transforms the control inputs F^ and F z into signals 
which are applied to motors at joints of the robotic arm 40 
that will result in the desired position and force vectors. 
The transformaton by a Jacobian matrix is well known 
in this art and requires no further description. The out- 
put, T, from the maxtrix 130 to manipulator 140 results 
in joint position shown as 9 into task descriptor 125. In 45 
a similar manner the force of the manipulator 140 results 
in a signal P s into task descriptor 125. 

We shall now address the problems of force and posi- 
tion control separately in Sections 1 and 2 and then 
integrate the results in Section 3. 50 


Generalizing this simple model to the m-dimensional 
force subspace {Z}, the dynamic behavior of the system 
in { Z } can be expressed by the differential equation 

M^.(t)+DoZ(t)'K e Z(t)=Fli) (2) 

where Z(t) is the mxl end-effector position/orienta- 
tion vector, M 0 is the symmetric positive/defmite m X m 
generalized mass matrix, D 0 is the mXm generalized 
damping matrix, K<. is the diagonal mXm generalized 
stiffness matrix and F z is the mXl force vector applied 
to the end-effector in the force subspace {Z}. The ele- 
ments of K e are the “equivalent” translational (force) 
and rotational (torque) coefficients of elasticity (stiff- 
ness) of the system in various directions in {Z}. The 
equivalent stiffness k e of the force/torque sensor with 
stiffness k sen and the environment with stiffness k env is 
given by 


k e —(k S en ^ + k env 1 ) ^ 

By an appropriate choice of the { Z } subspace origin, 
the mX 1 force/torque vector P(t) exerted by the end- 
effector on the environment is related to Z(t) by the 
generalization of Hooke’s law as 

m =K e Z(t) (3) 

From equations (2) and (3), we obtain 

AP(t)+BP(t)+P(t)=FM (4) 

where A=M 0 K e ~ 1 and B=D 0 K e _1 are mXm matri- 
ces. Equation (4) gives a simple dynamic model of the 
system in the force subspace {Z}. 

Since the manipulator dynamics are highly nonlinear, 
the matrices A and B in equation (4) are dependent on 
the end-effector Cartesian position and velocity vectors 
X and X and also on the system parameters such as the 
equivalent stiffness and the payload mass, which are 
represented by the parameter vector p. Furthermore, 
due to internal cross-coupling of the manipulator dy- 
namics, a “disturbance” term C/<Y) nust be included in 
equation (4) to represent the dynamic coupling from the 
position loop into the force loop, where Y is the end- 
effector position vector in {Y}. 

Taking the above into account, a more realistic model 
for force control is obtained as 

A(X±p)Kt)+B(X,X.p)P(‘)+ Ht)+ C p {Y)=FM (5) 


1. FORCE CONTROLLER 

In this section, a simple dynamic model for a force 
controller in the subspace {Z} is described and adaptive 
force control in accordance with the invention is de- 55 
scribed. 

1.1 Dynamic Force Model 

The full dynamic model of an end-effector plus for- 
ce/torque sensor in contact with the environment is 60 
complex. See, for example, Eppinger and Seering as 
noted earlier herein. However, the dynamic behavior of 
this system can be modeled approximately by a well 
known mass-spring-damper 200 in each degree-of-free- 
dom as shown in FIG. 2 and described by the differen- 65 
tial equation 

( 1 ) 


Equation (5) is a set of highly complex nonlinear and 
coupled second-order differential equations. 

1.2 Adaptive Force Control 

In order to control the force/torque P(t) exerted by 
the end-effector on the environment, a PID controller is 
employed with adaptive gains {K^(t), K/(t), Kz>(t) } and 
an auxiliary signal d(t) in the force control law as devel- 
oped herein. The force control law is 

r t (6) 

Flo = PM + K p (t) E(t) + KM E(t)dt + KM E(t) + 

J 0 

d(t) 

where P^t) is the mXl vector of desired force trajec- 
tory used as a feedforward term, and the mxl force 
tracking-error vector E(t)=PXt) — PX0 — P(t) is the 


mz(t) + dz(t) + kz(t) =J[t) 
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deviation of the actual (measured) force from the de- 
sired value. Since in practical applications the desired 
force trajectory is very often a constant setpoint 
P,(t)=Pr> the PID control law is particularly suitable 
for this situation. 5 

Furthermore, it should be appreciated that the auxil- 
iary signal d(t) compensates for the cross-coupling term 
C p and the time and parameter variations of A and B 
matrices. Note that the feedforward term P^t) is in- 
cluded in the control law (6) since ideally we want 10 
P(t)=P^t). 

In equation (6), the gains of the PID controller, 
namely K^(t), KXO and Ko(t), and also the auxiliary 
signal d(t) are adapted in real-time to accomplish force 
setpoint tracking in spite of the nonlinear and possibley 15 
time-varying behavior of the system model of equation 

(5). 

On applying the linear control law of equation (6) to 
the system model of equation (5), we obtain 


matrices, the force errors will be decoupled; for in- 
stance 

E mi(0 + mi(t) + dnE m ,(t) ~ d \ (E mi*(t) =0 ( 1 1 ) 

where the coefficients du, du and d3,- are chosen such 
that the tracking-error E,ft) = P ri -(t) — P,<t) has a desired 
behavior and d2/d3;>di,to ensure stability. 

Equation (10) can be written as 


0 dm x m (t) — D x m (t) 
-D\ -Di -£>3 


where x m (t) = E m (t) 


: PA. t) + K p E(t) + K, 


E(t)dt + 

KoE(t) + d(t) 


Using E(t)=P,(t)— P(t) and noting the E(t)= — P(t) and 
E(t)=— P(t) for a constant desired force, equation (7) 
can be written as 

E(t)+A-\B+K D )m+A-\l+K p )E(t)+A^K I . 
E*(t)=A~ l [C p -d(t )] ( 8 ) 


is the 3 mX 1 desired error vector. Equation (12) consti- 
tutes the “reference model” in the context of “MRAC” 
theory. Since the initial values of the actual and desired 
25 forces are often the same, the initial error x m (0) is equal 
to zero, and hence from equation (12), x m (t) = exp 
[Dt].x m (0)=0 for all t. 

Now, in order for the adjustable system state x e (t) to 
tend to the reference model state x m (t)=0 asymptoti- 
30 cally, from Gilbart, Monopoli, and Price, noted earlier, 
we require 


2 *W = f J i 
J o 


0 

35 o = Q-'Mx e + Q*oMx e 
„ A~ l d 


is the m+1 integral error vector. Equation (8) can be 
expressed in standard state-space form as 


-A-'Kj -A~\l + Kp) -a~\b + k d ) 


0 E\t) 

0 where x^r) = £(r) 

A-\C p - d) I l £(r) 


A-'Ki A~ x K d 


— Qi MxgX e 


ft l dm 0 
0 7i L 


+ S*1 ( Xe Xe) 0 


where denotes transposition, {ai, /3i, yi) are posi- 
tive scalars, {a2, Pi, 72} are zero or positive scalars, and 
L is an mXm constant matrix to be specified later. In 
equation (13), Q 0 and Q/are symmetric positive-definite 
3 mx3 m matrices, Q 0 * and Qi* are symmetric positive 
semi-definite 3 mX3 m matrices, and the symmmetric 
positive-definite 3 mX3 m matrix 


is the 3 mXl augmented error vector. Equation (9) 
constitutes the “adjustable system” in the Model Refer- 
ence Arm Control (“MRAC”) framework. 

Now, in the ideal situation, the desired behavior of 
the force error E m (t) is described by the homogeneous 60 
differential equation 

E m( l ) m U) + EiE , n (t) -x D \E (t) —0 (10) 

where Di, D2 and D3 are constant mXm matrices 65 
which are chosen such that the equation (10) is stable 
and embodies the desired performance of the force 
control system. By choosing Dj, D2 and D3 as diagonal 


(M 1 m 2 Mi 
M = iW*2 -^4 Ms 
. Mi Ms Me 


is the solution of the Lyapunov equation for the refer- 
ence model (12), namely 

MD+D'M=-N (14) 

where N is a symmetric positive-definite 3 mX3 m 
matrix. In deriving equation (13), the matrices A, B, and 
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Cp in the robot model of equation (5) are assumed to be 
unknown and “slowly time-varying” compared with 
the adaptation algorithm. This assumption is proper 
since these matrices cannot change significantly in each 
sampling interval, which interval is of the order of a 
millisecond. Now, in order to make the controller adap- 
tation laws independent of the model matrix A, we 
choose 


• A*; Qi = A*; Q * 0 = S 2 [A*\- 


It is noted that the auxiliary signal d(t) can be generated 
by a PI 2 D controller driven by the force error E(t) 
since, from equations (20)-(21), d(t) can be expressed as 


; d( 0) + [S2M6]£(f) + [Si + 8 2 Af5]£(f) + [SiMs + 


E(t)dt + [SiAf 3 ] 


is a symmetric positive-definite 3 m X 3 m matrix. Also 
note that A is a symmetric positive-definite matrix and 
hence A* and [A*] -1 are also symmetric positivie-defi- 
nite matrices. 

Substituting from equation (15) into equation (13), 
after simplification we obtain the adaptation laws 

0(f) = 8 1? (f)+ 829(f) (16) 


KM = cq[9(r) £'(f)] + a2^-[q(t) E*(t ) ] 


Q*i = [/l*]- 1 In practical implementation of any force control law, 

differentiation of the noisy force measurement P(t) is 
where {8i, 82} are positive and zero or positive scalars, 15 undesirable and, moreover, differentiation of the con- 
and stant force setpoint P r produces unwanted impulses. 

This argument suggests that the derivative E(t) in equa- 
tions (20), (24) and (25) must be replaced by — K e Z(t) 
(a 0 0 using equation (3). This yields the linear adaptive force 

A' = 0 A 0 20 control law 


FM = PM + d(i) + KM E{t)dt + 

Jo 

25 K p mo) - KMm 

which is shown in FIG. 3, where K v (t)=Kij(t)K e is the 
mXm velocity feedback gain matrix and the term 
K v (t)Z(t) represents velocity damping. 

30 FIG. 3 is the control loop for performing adaptive 
force control in accordance with the invention. FIG. 3 
is an adaptive controller capable of performing the 
control law of equation 26. In FIG. 3 the reference 
signal P r applied at lead 307 is fed forward via loop 308 
■ to a summing junction 320. The output from summing 
junction 320 is the signal F z which is an electrical signal 
that represents the force control law of equation 26. 

Summing junction 310 receives the reference signal 
^ P z and combines it with the actual force signal P that is 
fed back after measurement by a force sensor at the end 
effector, via feedback loop 315. An error signal E from 
summing junction 310 is applied to two separate vari- 
able gain circuits 332 and 333. 

45 Gain circuit 332 connected in lead 312 between sum- 
ming junctions 310 and 320 represents the term K^. 
Input signal E is applied to the variable K p gain circuit 
332 which varies in time as controlled by adaptation 
control 335. 

50 Adaptation control 335 also controls a second vari- 
able gain circuit 333 which is connected in lead 313 
between 310 and 320. An integration circuit 334 re- 
ceives the error signal E, integrates it, and delivers an 
integral of the error, signal E*, to gain circuit 333. The 
55 output from the variable gain circuit 333 is an input 
term to summing junction 320. 

Adaptation control 335 receives three input terms, 
namely error E, integral E* and Z. The term Z is a 
measured velocity term which indicates how fast the 
60 end effector is moving into the manipulated object as 
the force is being applied. Z is also fed through another 
variable gain circuit 342 and the modified Z term is also 
fed into summing junction 320. Note that this position- 
related term is in the adaptive force controller and it 
65 assists in the cross-coupling stability feature of this in- 
vention. 

Choosing L=(K t ro ') 2 and replacing for L in equation 
(19), the adaptation laws now become 


Kp(t) = 0 1 [9(f) E(t)] + 02^ [9(f) £’«] 


Kd( 0 = 71 [?« EM] + 72 ^ [«(') E(i)L] 


where q(t) is the mXl “weighted”force error vector 
defined as 

9(0=M 3 £*(f)+M 5 £(r)+Ar6£(t) (20) 

and M3, M5 and M6 are appropriate submatrices of M. 
Thus, the required auxiliary signal and PID controller 
gains are obtained as 


d(t) = d( 0 ) + 81 q(t)dt + 829(f) 
0 


KM = KAO) + a, 9(f) E-{t)dt + a 2 q(t) E*(l ) 
0 


K p (f) = £ p (0) + /3, 9(f) E\t)dt + 029(f) E\t) 

0 


KM = Kn( 0 ) + 71 9(f) E'(t)Ldt + 729(f) E{t)L 

0 


The force control law is then given by 


f (25) 

FM = PM + d(i) + KAt) E(t)dt + Kp(t)m + KME{t) 

0 
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d(t) = rf( 0 ) + 8 , f ‘ 
Jo 

q(t)dt + 829(0 

(27) 

KK 0 = KX 0 ) + ai J 

' q(t)E'(t)d, + aiqm v (t) 

0 

(28) 

K p (t) = Kp(P) + Pi ^ 

( ‘ qmV)dt + Piq(0E\‘) 

b 

(29) 

Arxo = *XO) + ri J. 

- t 

q(t)Z\t)dt - 72 q(<)Z'(t) 

0 

(30) 

where 



q(t)=M,EXt)+M 5 E«) 

-4f 6 *Z(r) 

(31) 


12 

able and yields improved dynamic performance. The 
adaptation laws (27) through (30) do not require the 
complex nonlinear model of manipulator dynamics (5) 
or any knowledge of parameters of the manipulator or 

5 the environment. This is due to the fact that the adapt- 
ive force controller has “learning capabilities” and can 
raidly adapt itself to gross changes in the manipulator or 
the environment parameters. 

10 2. POSITION CONTROLLER 

In this section, a dynamic model for adaptive position 
control in the subspace {Y} is described. 

2.1 Dynamic Position Model 

15 The dynamics of the end-effector in the Cartesian 
space {X} can be represented as discussed in O. Khatib 
as noted earlier herein. The dynamics are represented as 


and M6* =M6K e . By proper selection of matrix N in the 
Lyapunov equation (14), the submatrices M3, Ms and 
M6 in equation (20) can be made equal to the desired 
values W/, W p and Wo=W,K e - 1 respectively, and 
hence equation (31) becomes 

q(t)= W,E\U)+ WpE(t)— W t Z(t) (32) 

where W/, and W p and W v are the mXm diagonal 
weighting matrices chosen to reflect the relative signifi- 
cance of the integral error E*, the force error E, and the 
velocity Z, respectively. 

Adaptation control 335 synthesizes a signal d which 
is also applied to summing junction 320. Equations (27) 
through (30) are performed by the adaptation control 
335. Adaptation control 335 may be any suitable device 
such as a program in a memory which, upon input of 
terms E, E*, and Z converts them into the equations 
(27) through (30). Note that each of these equations 
vary with time and, when applied to the variable gain 
circuits, also vary the gain in time. 

From equations (27)-(30), the general expression for 
a typical controller gain K(t) which acts on the signal 
v(t) to generate the term K(t)v(t) in the control law (26) 
can be written as 

r t (33) 

K{t) = K( 0) + m I q(t)v'(t)dt + H2 9(0' l '(f) 

Jo 


\(X)X+$(X, X)+^x)+P=F (35) 

where A is the Cartesian mass matrix, cj> is the Cartesian 
centrifugal, Coriolis and friction vector if) is the Carte- 
sian gravity loading vector, P is the force vector ex- 
erted by the end-effector on the environment, and F is 
the generalized “virtual” Cartesian force vector applied 
to the end-effector. In the position subspace (Y }, equa- 
tion (35) can be written as 

A(X, X, p)Y(t)+B(X, X, p)Y(t)+C 0 (X, x, 
p)Y(t)+C/P)=F/t) (36) 

where the lxl matrices A, G, C 0 are highly complex 
nonlinear functions of X, X and the system parameters 
p, C/ represents the dynamic coupling effect from the 
force loop into the position loop which is a function of 
the force vector P in {Z}, and F^is the IX 1 force vector 
applied to the end-effector in the position subspace {Y}. 
Equation (36) is a set of highly complex nonlinear and 
coupled second-order differential equations. 

2.2 Adaptive Position Control 

The linear adaptive position control law in accor- 
dance with this invention is given by 
F/t) =M +Kp(.t)E p (t ) + _ .. 
t)E p (t) + C(t)R(t) + B(t)R(t) +A(t)R(t) (37) 


20 


25 


30 


35 


40 


45 


where p.; and p.2 are scalar gains. 

The adaptive force control of this invention is ex- 
tremely simple, since the adaptation laws (27) through 
(30) generate the controller gains by means of simple 
integration using, for instance, the trapezoidal rule; e.g. 
equation (33) can be implemented as 


where R(t) is the lxl reference (desired) position tra- 
jectory vector, E p (t)=R(t)— Y(t) is the 1X1 position 
tracking-_error vector^ f(t)_is an auxiliary signal, and 
[KpEp+KyEp] and [CR+BR-f AR] are the contribu- 
tions due to the feedback and feedforward controllers 
respectively. The required auxiliary signal and control- 
ler gains are adapted according to the following laws: 


50 


T s (34) 

m = *(i - 0 + mi • ~y~ [<?('>'« + 

9(1 - - 01 + H2 <?('>'(0 

where the integer i denotes the sampling instant and T s 60 
is the sampling period. As a result, the force control law 
of equation (26) can be evaluated very rapidly and con- 
sequently, force control is implemented for real-time 
control with high sampling rates (typically 1 KHz). 

While a sampling source is not shown in FIG. 3, it is 65 
to be understood that all components are sampled and 
synchronized for a real-time adaptive control. The high 
sampling rate of real-time adaptive control is very desir- 


At) =/(0) + 8 , ( 

Jo 

f 

r(t)d‘ + 82 KO 

(38) 

K p (t) = K p ( 0) + v, 

f ' MEpXOdt + vzAt) E p '(t) 

Jo 

(39) 

KM = KdP) + VI 

f ii‘)EpXt)dt + VI MEpXt) 

Jo 

(40) 

C(t) = C(0) + HI j 

f 1 dt)RXt)dt + H2 <t)RX‘) 
0 

(41) 
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-continued 



f 1 

(42) 

m = m + ri 

WW + 72K0*'(0 
-'0. 



r ' 

(43) 

A{t) = A(0) + X, 

| Kt)R'(t)dt + \ 2 g{t)RV) 
Jo 



where the 1 X 1 “weighted” position error vector r(t) is 
defined as 

H.t)=w p E f k)+w s Ej,i) (44) 10 

in equations (38)-(43), {Si, v), gi, pi, Yi, Xi) are posi- 
tive scalars, { 82 , V2, rji, P2, 72, ^2} are positive or zero 
scalars, and WPhd p and W v are weighting matrices 
reflecting the relative significance of the position and 1 
velocity errors E p and E p . 

The position control scheme is extremely simple since 
the controller gains are obtained from equations (38) 
through (43) by simple integration (such as the trapezoi- 
dal rule) and thus the computational time required to 20 
evaluate the position control law (37) is extremely 
short. Thus, the adaptive position control of this inven- 
tion is implemented for on-line control with high sam- 
pling rates (~1 KHz); resulting in improved dynamic 
performance. 25 

3. HYBRID FORCE/POSITION CONTROL 
SYSTEM 

In Sections 1 and 2, the Cartesian end-effector forces 
F 2 and Fj, are generated by the force and position con- 20 
trailers to accomplish force and position tracking in 
{Z} and {Y} respectively. Since Cartesian forces can- 
not be applied to the end-effector in practice, these 
end-effector forces must be mapped into the equivalent 
joint torques. Thus, in order to implement the force and 35 
position controllers, the control law in joint space is 
given by J. J. Craig (37 Robotics-Mechanics and Con- 
trol,” Addison-Wesley Publishing Company, Reading, 
MA, 1986). 

40 

where is the nX 1 joint angle vector, T is the nX 1 
joint torque vector, and J is the nXn Jacobian matrix of 45 
the manipulator, with appropriate reordering of the 
columns of J if necessary. 

In FIG. 4 the desired position, R, is a signal from the 
trajectory generator 105, FIG. 1. That signal R is a 
measured value at the end effector, and it is applied to a 50 
summing junction 410 where it is compared with the 
actual position Y. The other two input terms R and R 
are also specified by the trajectory generator and repre- 
sent desired velocity and desired acceleration. These 
input terms thus fully determine what kind of move- 55 
ment at what speed and how quickly the end effector is 
to be controlled. 

In a manner very similar to that described earlier for 
FIG. 3, the adaptation control 405 performs the position 
control equations (36) through (44) by controlling the 60 
gain of the various variable gain amplifiers as shown in 
FIG. 4 for the appropriately labeled portions of the 
position control law of equation (37). Thus, position 
adaptation control 405 controls the variable gain units 
420, 430, 440, 450, 460 to input respectively to summing 65 
junction 470, position feedback gain, position feedfor- 
ward gain, accelaration feedforward gain, velocity 
feedforward gain and velocity feedback gain. Note that 


14 

Y is not a directly measured term but, in a manner for Z, 
is an indirectly derived term computed from the joint 
encoders in any well-known manner. Adaptation con- 
trol 405 also supplies the auxiliary signal f which is a 
synthesized position control term as shown in position 
control law (37). 

It is important to note that although the force and 
position controllers are separate in the hybrid control 
architecture, there exists dynamic cross-coupling from 
the force control loop into the position control loop and 
vice versa. This coupling is due to the fact that the 
end-effector dynamics in the Cartesian space {X} is 
strongly cross-coupled; i.e. the application of end-effec- 
tor force in any direction affects the end-effector posi- 
tions in all directions. The cross-coupling effects are 
modeled as “disturbance” terms C p and C/in the force 
and position control laws and related control loops. 

The adaptive controllers of this invention compen- 
sate for these disturbances and maintain a good tracking 
performance. The ability to cope with cross-coupling 
effects in the hybrid control architecture of this inven- 
tion is an important feature. 

FIG. 5 is a generic module which is readily adaptible 
for achieving the adaptive gain terms for the force/posi- 
tion control laws of this invention. In FIG. 5, the input 
term v(t) can be any of the variables shown in the con- 
trol law equations (26) or (37). Note, for this example, 
that the last term of force control equation 26 is the 
output term from module 500. The input term q(t) was 
defined earlier herein at equatiions (31), (32), etc. Input 
term v(t) is -transformed or reorganized by transforma- 
tion unit 510 and is applied to a multiplier unit 520. The 
remainder of the module is self explanatory and needs 
no further explanation. Suffice it to say that each mod- 
ule 500 for the specific terms of the adaptive force/posi- 
tion control laws is used in the variable gain units such 
as the five units 420, 430, 440, 450, and 460, FIG. 4. In 
addition another module unit in adaptation control 405 
generates the auxiliary term f. The same holds true for 
the adaptive force control functions as depicted and 
described by FIG. 3. Since the modules are operating in 
parallel, high speed solutions to the simple arithmetic 
operations of this invention are readily achieved. 

4. ILLUSTRATIVE EXAMPLE 

The adaptive force/position control of this invention 
is applied to a two-link planar manipulator to control 
the motion and the exerted force of the end-effector 
simultaneously. The results of that application is now 
described. 

Consider the two-link manipulator depicted in a ver- 
tical plane in FIG. 6. It is assumed the manipulator’s 
end-effector 605 is contacting a vertical reaction surface 
610. The dynamic model which relates the joint torque 
vector 

-ft) 

to the joint angle vector 

-ft) 



4 , 860,215 


is well known. Such a model, described by J. J. Craig, 
as noted erlier noted, is given as: 

7W=M0)0+AT(», «) + C(«)+/(W) (46) 

where the above terms are defined as: 

Inertia matrix 


a\ + Q2 CQS ^2 


«3 + (<12/2)COS02 


03 + (O2/2)COS02 03 


Coriolis and centrifugal torque vector 


1V(0,0) = 


— (a2Sin02)(0102 + ®2 2 /2) 
(a2Sin02) 01 2 /2 


Gravity loading vector 


/ a^cosS\ + a 5 cos( 0 i + 02 )' 


a5COS(0) + 02) 


Jacobian matrix 


— /isin0i — / 2 sin( 0 i + 02) — / 2 sin( 0 i + 02 ) 

/icos0i + /2COS(01 + 02) /2COS(01 + 02) 


End-effector force vector 


■(?)■ 


0 if x < x 0 (no contact) 
/?e[x(0 - x 0 ] if x ^ x 0 
(during 
contact) 


simulate the robot behavior and are not used to generate 
the control action; i.e. the system dynamics in accor- 
dance with this invention is treated as a “black-box” by 
the adaptive force and position controllers 120, 110, 
FIG. 1. 

Suppose that the end-effector is required to apply a 
specified constant force setpoint P r on the reaction sur- 
face in the x-direction and simultaneously track a given 
position trajectory y 4 (t) in the y-direction; where 


^X')=0-432[1X6 exp (— 1/0.3) — 8 exp ( — f/0.4)] 


with y^0)= -0.432, yXoo)=0.432 and y^O- 

)=y^oo)=0. 

Using the adaptive hybrid control system of this in- 
vention, the two-dimensional Cartesian space {X} is 
decomposed into two orthogonal one-dimensional force 
subspace {x} and position subspace {y}. For each sub- 
space, a separate adaptive controller designed in accor- 
dance with this invention is as follows. In this example, 
the base frame 630 is chosen as the constraint frame. 

Force Controller: In accordance with Section 1, a 
reference model for the force tracking-error 
e x (f)=P^t)—P x (j) is chosen as 

e x (l) + 30 d x (t) + 300e x (l) + 1000e x *(t) = 0 

with three characteristic roots at — 10; yielding 
Di = 1000, D2=300, and D 3 = 30. The linear adaptive 
force control law is given by 


F x {t) = pm + d(i) + KM f ' e x (t)dt + K„(t)eM 
J 0 


KMm 


where K e is the stiffness of the reaction surface, X 0 is the 
location of the surface, and 

x(t)=l\ cos 01+/2 cos ( 01 + 02 ) 45 

is the horizontal coordinate of the end-effector in the 
base frame generated by the task descriptor 125, FIG. 1. 
The task descriptor 125 also generates the end-effector 
vertical coordinate y(t) from the joint angles 0\ and 02 50 


X0=h sin 01 + /2 sin (01 + 02 ) 

In the above expressions, ai, . . . , as are constant param- 
eters obtained from the masses {mi, m 2 } and the lengths 55 
{1]> I 2 } of the robot links 600A and 600B, including the 
end-effector 605. For the particular robot under study, 
the numerical values of the link parameters are chosen 


Mj = 15.91 kg, m 2 = 1 1.36 kg, h =12—0.432 meter 

so that they represent links 2 and 3 of the commercially 
available Unimation PUMA 560 arm. The reaction 
surface location is x 0 = 0.432 meter and its stiffness is 65 
chosen as K3=10 4 Nt/meter. It should be understood 
that the mathematical model and the parameter values 
of the robot and the reaction surface are used merely to 


where F* is the control force applied to the end-effector 
in the x-direction. The adaptive force controller gains 
are adapted as follows: 


d(t) = ?(/) + q(i)dt 


KM =1000 + 100 f q(t) e x \i)dt 

do 

K p (t) = 100+10 f q(t) e x (i)dt 
JO 

KM = (5) 10 3 - (4) 10 4 J ‘ q(t ) i(f)dt 
J 0 

where 

q(t) = 2e x *(t) + 0.25e*tt - 100x(r) 

and 

<?x'(0 = f e x (t)dt 

JO 

The integrals in the above equations are computed using 
the trapezoidal integration rule with dt=0.5 msec. 
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Position Controller: In accordance with Section 2, a 
reference model for the position tracking-error e y — 
yM —y(t) is chosen as 

e-J.Q+20eJ.t)+l00ey(t)=0 

with natural frequency to =10 rad/sec and damping 
factor | = 1; yielding Di = 100 and D2=20. The linear 
adaptive position control law is given by 

FJf) =A‘) + Xptityi) + F/Oe/O + C(t)yM + B(i- 
)+MyM (4*) 

where F y is the control force applied to the end-effector 
in the y-direction. The adaptive position controller 
gains are adapted as follows: 


*/) e^tyt 


KA 0 = 2 


At) = 0.5*/) + 0.5 f ‘ , 
■>0 

K p (t) = 2 f */) e/tyt 

JO 

KA t) = 2 f ( */) efiyt 
Jo 

C(/) = 0.5 [\t)yAty 
Jo 

B(t) = 0.5 f *t) yA>y 

Jo 

*(o=o.5 r^oMo* 
Jo 


*/) yA‘)dt 


*0 


*0 yAfyt 


*t) = 500 e)(/) + 200 e/f) 

The integrals in the adaptation laws are evaluated using 
the trapezoidal rule with dt=0.5 msec. 

Hybrid Control: The force and position controllers 
described above operate in the adaptive hybrid control 
system of FIG. 7. The joint control law is then given by 


7X0 = f(0) 


(FA0 \ 
\F/f> ) 


To evaluate the performance of the adaptive force/- 
position control schemes, the nonlinear dynamic model 
of the manipulator (46) and the adaptive hybrid control 
law (49) were simulated on a DEC-VAX 11/750 com- -0 
puter with the sampling period of 0.5 msec. The results 
of this simulation are shown in FIGS. 8a and 8 b and 
indicate that P x (t) and y(t) track the desired trajectories 
PKt) and yr(t) very closely. Thus the adaptive force and 
position controllers of this invention compensate for 55 
system non-linearities in a highly satisfactory manner. 

The performance of the adaptive controller system in 
four different cases was tested by the above-noted simu- 
lation and the results of these four cases are discussed as 
follows. 60 

Case 1 — Performance With No Vertical Motion 

In this case, the end-effector is required not to move 
in the y-direction (i.e. yr(t) = — 0.432 meter) and exert 
the constant force of P*t)=10 Nt on the reaction sur- 65 
face using the same control law (49). FIGS. 9a and 9b 
show the responses of the exerted force P*(t) and the 
vertical coordinate y(t) and indicate that force tracking 


is accomplished and the maximum deviation of y from 
its initial (desired) value is less than 1 mm. The effect of 
cross-coupling between force and position control 
loops can be observed by comparing FIGS. 8a and 9a. 

Case 2 — Performance under Stiffness Variation 

In order to test the learning capability of the force 
controller, the stiffness of the reaction surface is sud- 
10 denly changed to K e =(5)10 3 Nt/meter at t= 1.5 sec- 
onds while the end-effector is exerting the force of 10 
Nt on the surface under the adaptive control law (47). 
This abrupt change is a clear violation of the assumption 
made in the theory that the system parameters are 
15 “slowlly time- varying.” In spite of this violation, the 
adaptive force controller invention continues to per- 
form remarkably well. FIG. 10 shows the response of 
the exerted force P*(t) and indicates that the desired 
force setpoint P*t)= 10 Nt is tracked closely despite the 
sudden change in K e . Thus, the adaptive force control- 
ler is not unduly sensitive to reaction surface stiffness. 

Case 3 — Performance With Reaction Surface 
Movement 

In this case, the end-effector is required to maintain 
the constant force of 10 Nt on the reaction surface while 
the surface is moved away from the end-effector in the 
positive x-direction with the constant speed of of 1 
30 cm/sec in the time interval 0.5 <t <2.25 seconds. The 
response of the exerted force P x (t) and the position of 
the reaction surface x 0 (t) are shown in FIGS. 11a and 
11 b. It is seen that the constant force of 10 Nt is exerted 
3 5 on the surface despite the surface movement. The nega- 
tive transient in P*(t) at t=0.5 and the positive transient 
at t= 2.25 will be reasoned by those skilled in this art 
simply from the physics of the situation. 

Case 4 — Complaint Motion 

In previous cases, it is assumed that the end-effector is 
the active element in force exertion. Let us now con- 
sider the reverse situation where the end-effector is the 
passive element and force is exerted by the reaction 
45 surface on the end-effector. In this case, suppose that 
the surface is moved towards the end-effector in the 
negative x-direction with the constant speed of 1 
cm/sec in the time interval 0.5 <t <2.5 seconds, and the 
5 0 end-effector is required to comply with the surface 
movement while maintaining a small contact force of 
P^t)=0.1 Nt. The end-effector will now reposition 
itself under the force control law (47) to maintain the 
desired force of 0.1 Nt; otherwise the contact force 
55 created will reach 200 Nt. 

FIGS. 12a and 126 show the response of the actual 
force on the end-effector P_ l (t) and the reaction surface 
position x 0 (t). It is seen that the end-effector complies 
completely with the force applied by the surface and 
the actual end-effector force varies about the desired 
value of 0.1 Nt. 


5. SOLUTION OF THE LYAPUNOV EQUATION 

In this section, we obtain explicit closed-form solu- 
tions of the Lyapunov equation for a particular class of 
stable systems. 

Let us consider the (3m)th order stable system 
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= 


f: 


Im 

0 



i^-Di -D 2 - D 3 j 


Ql = DiW/ 

(55a) 

Ql = -D}W I+ D 2 W p -D\ W D 

(55b) 

Qi=-w p +DiW d 

(55c) 


where Di, D 2 and D 3 are mXm diagonal matrices with 
positive elements such that D 2 D 3 >Di. Let the 3mx3m 
matrix 10 


For the Q, matrices to be positive-definite, the following 
inequalities must hold 

(DiWi+D\W d )< D 2 W p < D2D3 W D (56) 


p = 


'P\ Pi Pi' 
Pi Pi Pi 
v Pi P 5 Pi y 


15 


be the symmetric positive-definite solution of the 
Lyapunov equation 


'P\ Pi Pi' 

'0 

Im 0 N 


fO 0 -Di \ 

(51) 

Pi Pi Pi 

0 

0 I m 

+ 

Im 0 D 2 


s Pl Pi Pi y 


— D 2 -Dl y 


^0 I m D3 y 




' P\ Pi Pi ' 


(-2 Qi 0 0 N 



Pi Pi Pi 

= 

0 -2Q 2 C 

> 



v Pi Pi Pi y 


^0 0 

-2G3 ; 


where Pi, . . . , P 6 are mXm diagonal matrices, and Qi 
and Q 2 , Q 3 are mXm diagonal matrices with positive 
elements. From equation (5 1), we obtain the set of equa- 
tions. 


—2PiD\ — —2Q\ 

P\-PlDi-P s D\ =0 

Pl-PlDi-PeD^O 
2Pl-2PiDi = -2Qi 

Pi — Pffii + Pi - DiP$ =0 

2P 5 -2P(fii = -2Qi 


(52a) 

(52b) 

(52c) 40 
. (52d) 

(52e) 

(520 45 


The solutions of these equations for submatrices P 3 , P 5 , 
and P 6 are found to be 

Pl~D\~ l Q\ (53a) 5o 

P$ = D4~ l[(Dl - 1 £3 2 )<2i+(0 3 )G2-KDi)e3] (53b) 

P 6 =Di~ •((£>! - >i)3)ei + Ql+(D 2 )Qi] (53c) 

where Di=DiDi—D\. Equations 53(a)-(c) give simple 55 
closed-form expressions for certain submatrices of the 
solution of the Lynapunov equation (51). Different 
choices of Qi, Q 2 , and Q 3 will generate diferent solu- 
tions P of the Lyapunov equation. 

Often we wish to choose the matrices Qi, Q 2 , and Q 3 60 
such that the acquired solution P has som desired sub- 
matrices. For instance, in order to obtain a solution with 


P 3 =w r , P 5 =w / , ; P 6 =W D (54) 

where W /, W p and W jj are some specified diagonal 
matrices, from equations 52(a)-(f), the required Q,- ma- 
trices are obtained as 


6 . NON-ADAPTIVE EMBODIMENT 

The adaptive position control described hereinbefore 
does not require knowledge of the manipulator dynam- 
ics. If such knowledge is available, the inventor has 
described a non-adaptive robust control scheme herein 
in FIG. 136 and in his paper entitled “Linear Multivari- 
able Control of Robot Manipulators” presented to the 
IEEE during the week of Apr. 7th through 10th, 1986. 
A copy of that paper is submitted herewith and its dis- 
closure is incorporated herein by reference pursuant to 
M.P.E.P. Section 608.01(p). More particularly the in- 
corporation by reference includes the equations and 
material that describe the components of FIG. 13a and 
136 herein and the improvement over the basic robot 
control scheme depicted in FIG. 13a. Both figures are 
described in detail in the inventor’s paper. 

FIG. 136 includes a feedforward loop 1300 which 
contains only the proportional-double-derivative terms 
as depicted. Feedforward controller 1300 is designed to 
be the inverse of the manipulator’s dynamics. The feed- 
forward terms ensure that the manipulator joints follow 
desired reference trajectory from a trajectory generator 
as earlier described. 

Feedback controller 1320 of FIG. 136 is independent 
of the feedforward controller 1300. Although each 
controller 1300 and 1320 is multivariable, the feedback 
controller 1320 achieves pole placement and ensures 
zero initial tracking error by the proportional-integral- 
derivative terms depicted therein. 

The non-adaptive controller of FIG. 136 may more 
readily be understood by assuming that the gain circuits 
of the adaptive position controller as fully described 
herein are set and fixed. A non-adaptive controller of 
FIG. 136, as claimed, ensures position tracking in real 
time, and does so with a highly simplified design ap- 
proach. 

7. SUMMARY 

A new and novel method and apparatus for achieving 
high speed real-time force/position control of a manipu- 
lator has been disclosed and claimed. The adaptive 
control law for force, as established by equation (26), is 
performable at high speeds in a digital hybrid control 
architecture. The adaptation of this invention is also, of 
course, readily performed in suitable analog methods 
and functions. Adaptive or non-adaptive position con- 
trol alone, or in combination with, the adaptive force 
control as described and claimed herein provides new 
and heretofor unattained results. 

The above description presents the best mode con- 
templated in carrying out the invention. The invention 
is, however, susceptible to modifications and alternate 
constructions from the embodiments shown in the 
drawings and described above. Consequently, it is not 
the intention to limit the invention to the particular 
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embodiments disclosed. On the contrary, the invention 
is intended and shall cover all modifications, sizes and 
alternate constructions falling within the spirit and 
scope of the invention, as expressed in the appended 
claims when read in light of the description and draw- 5 
ings. 

What is claimed is: 

1. In a hybrid controller adapted to issue a manipula- 
tor control signal which controls a manipulator’s posi- 
tion and force in Cartesean space, wherein the manipu- 10 
lator and its environment form a system exhibiting dy- 
namics which are non-linear and also include manipula- 
tor and system parameters which are not known to a 
user who controls the manipulator in Cartesean space 
through a user-initiated position and force trajectory 15 
command that is received at the hybrid controller, and 
wherein the hybrid controller senses force and position 
of said manipulator in said Cartesian space, the im- 
provement comprising: 

manipulator driving means responsive to said control 20 
signal issued by said hybrid controller for driving 
said manipulator in said environment to achieve, in 
Cartesean space said desired position and force 
indicated by said trajectory command received by 
said hybrid controller, which command is issued to 25 
said hybrid system free of a priori knowledge of 
any of said non-linearities and/or the manipulator’s 
and/or system’s parameters; and 

adaptive control means, responsive to said sensed 
force and position, for varying said control signal 30 
applied to said manipulator’s driving means, which 
variable control signal compensates in real-time for 
the system’s nonlinearities as said manipulator is 
driven in Cartesean space to said commanded posi- 
tion and force. 35 

2. The hybrid controller in accordance with claim 1 
wherein said adaptive control means is further charac- 
terized as comprising: 

an adaptive force control which includes a sensed 
force signal feedback circuit and variable feedfor- 40 
ward signal generating means for receiving a force 
command portion of said received command and 
modifying said force command portion by said 
sensed force signal of said force feedback circuit; 

an adaptive position control circuit which includes a 45 
position signal feedback circuit and variable feed- 
forward signal generating means for receiving a 
position command portion of said received com- 
mand and modifying said position command por- 
tion by said sensed position signal of said position 50 
feedback circuit; and 

means for connecting both said force and position 
signal generating means to said manipulator driv- 
ing means in order to control said manipulator’s 
position and force exerted on said load or said 55 
environment in said Cartesian space. 

3. A hybrid controller in accordance with claim 2 
wherein the manipulator driving means is further char- 
acterized as including; 

a signal summing junction for force control connect- 60 
able to said manipulator for applying thereto a 
combined variable force control signal derived, at 
least in part, from said feedforward and adaptive 
feedback control loops. 

4. A hybrid controller in accordance with claim 3 and 65 
further comprising: 

variable gain control circuits in said adaptive force 
control loop; and 
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an adaptation control responsive to signals indicative 
of the manipulator’s actual Cartesian movements in 
said environment for controlling said gain control 
circuits in order to compensate for the system’s 
non-linearities. 

5. A hybrid controller in accordance with claim 4 and 
further comprising: 

a position and force trajectory generator emitting 
force reference and/or position trajectory refer- 
ence command signals; and 

means for comparing the desired position and/or 
force reference command signal(s) and an actual 
force signal representing the Cartesian force ap- 
plied by said manipulator to said load or environ- 
ment, in order to derive an error signal for applica- 
tion to said feedback control loops. 

6. A hybrid controller in accordance with claim 5 
wherein said signal summing means further comprises: 

a signal summing junction for summing said desired 
force reference command signal with said actual 
manipulator Cartesian force signal to derive there- 
from said real-time force control signal. 

7. A hybrid controller in accordance with claim 6 and 
further comprising: 

means connecting said feedforward loop between 
said position and force command generator and 
said signal summing junction for feeding said de- 
sired force reference signal forward to said signal 
summing junction. 

8. A hybrid controller in accordance with claim 3 and 
further comprising; 

proportional-integral-differential (PID) controller 
circuits in said feedback force control loop. 

9. A hybrid controller in accordance with claim 1 and 
including a position and force command generator for 
emitting signals indicative of a desired position and 
force on said load and further wherein each manipula- 
tor’s actual force and position applied to said load are 
sensed in order to derive position and force terms that 
are thereafter respectively represented as error signals 
to be fed back and used by the hybrid controller, and 
wherein the improvement is further characterized in 
that: 

said adaptive control means comprises; 

a signal summing means for forming said real-time 
force and position control signals; 

a feedforward command control loop for applying 
said desired force and position command signals to 
said summing means; 

an adaptive force and an adaptive position feedback 
control loop also connected to said summing means 
for respectively receiving said desired force and 
position command signals; 

adaptation control means connected in said adaptive 
force and said adaptive position feedback control 
loops and responsive to the respective command 
signals thereof and also responsive to their respec- 
tive actual sensed error signals for modifying the 
commands in real-time in accordance with said 
error signals; and 

means for applying the so-modified command signals 
from said adaptation control means to said sum- 
ming means. 

10. A hybrid controller in accordance with claim 9 
and wherein said signal modifying means in said adapt- 
ive control means operates in accordance with a force 
control law which may be considered as though it was 
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applied directly to the manipulator’s end effector in 
Cartesian space, and said controller further comprises: 
an adpative force proportional-integral-differential 
(PID) controller in said force feedback control 
loop; 5 

a means connected in said force feedforward control 
loop for receiving a feedforward signal represent- 
ing a desired mathematical force term for said ma- 
nipulator; and 

a plurality of variable gain circuits for implementing 10 
a force control law characterized as: 


wherein the term R(t) is a desired position_trajec- 
tory, E p (t) is a position tracking error, [K p E / ,+- 
kvEJ and [CR+BR+AR] are terms contributed 
by feedback and feedforward adaptive position 
control loops; and 

wherein said force control law is characterized as: 
FM = PM + d(t) + KM f ' E(t)dt + 

Jo 


FM = PM + d(t) + KM f ‘ E(t)dt + 

Jo 

K p (t)E(t) - KM Z (') 

wherein P^t) is said desired force command signal 
term; d(t) is, at least in part, a cross-coupling posi- 
tion-related compensating term; the K/, K p , and K v 
signal terms are individually varied gain control 
terms; E is a force error term which is obtained by 
modifying the desired force command signal term 
by the actual force signal that is sensed at the ma- 
nipulator and Z is a position-related velocity damp- 
ing term which represents the manipulator’s veloc- 
ity in Cartesian space. 

11. A hybrid controller in accordance with claim 10 
and wherein said signal modifying means in said adapt- 
ive control means operates in accordance with a posi- 
tion control law which may be considered as though it 
was applied directly to the manipulator’s end effector in 
Cartesian space, and said controller further comprises: 

an auxiliary force signal for controlling, at least in 
part, the amount of force to be applied by said 
manipulator; and 

summing means for adding said auxiliary force con- 
trolling signal in combination with signals in said 
adaptive force feedback and force feedforward 
control loops. 

12. A hybrid controller in accordance with claim 1 
and wherein said improvement is further characterized 
by: 

means for compensating for dynamic cross coupling 
which results from adaptively controlling both 
position and force of said manipulator. 

13. A hybrid controller in accordance with claim 1 
wherein said adaptive control means further comprises; 

an adaptive position controlling means including 
adaptive position feedforward and adaptive posi- 
tion feedback control loops which include a plural- 
ity of variable gain circuits for implementing a 
position control law characterized as: 

FJf) =M + J p ( OEM 1+ F+ 

t)EfM + C(t)R(l) + BW(t) +A(t)m 

wherein the term R(t) is a desired position trajec- 
tory command signal tenn, E p (t) is aposition track- 
ing error term, [KpEp+KyE^] and [CR+BR+AR] 
are variable gain terms contributed by the feedback 
and feedforward adaptive position control loops. 

14. A hybrid controller in accordance with claim 1 
wherein said positioning of said load is controlled by a 
control law characterized as: 

Fy(t)= f(r) +J p (t)Ep(t)+ K+ 
t)E p (i) + C(t)R(t) + B(t)R(t)+A{t)R(t) 


Kp(t)E(t) - KM Z (t) 

wherein P^t) is said desired force reference signal 
term, d(t) is an auxiliary force signal term emitted 
by said adaptation control, and the K /, K p , and K v 
are variable gain terms; E is an error term; and Z is 
a velocity term. 

15. A hybrid controller in a system for controlling the 
positioning of, and force exerted by, a controlled manip- 
ulator in accordance with a received command signal, 
said system characterized in that the manipulator and 
the environment form a complex, dynamic model speci- 
fied as having unknown parameter values, and the sys- 
tem is further characterized by: 

adaptive control means for issuing a real-time vari- 
able signal to said manipulator to be controlled, 
which signal controls the position and force of said 
manipulator, said adaptive control means further 
comprising; 

means for sensing the actual force and positioning of 
said manipulator as it is being controlled; and 

variable control signal generating means connected 
to said sensing means for modifying said received 
command by said sensed force and position in 
order to supply said variable control signal issued 
from said adaptive control means without knowl- 
edge of said parameter values of the system. 

16. The hybrid controller in accordance with claim 

15, and wherein said manipulator exhibits a force re- 
sponse on a load or on said environment and includes a 
force driving means that is responsive to a force com- 
mand portion of said received command signal, and the 
improvement is further characterized by: 

said adaptive control means including at least one 
adaptive force controller for controlling said ma- 
nipulator’s force driving means in the force that it 
applies to said load or to said environment; and 

said adaptive force controller includes a real-time 
force signal generating means for emitting a force 
control signal that causes said manipulator to ex- 
hibit the desired force that is indicated by said force 
command portion of said received command signal 
either independently of, or simultaneously with, 
said position control over said manipulator. 

17. The hybrid controller in accordance with claim 

16, and wherein said manipulator additionally exhibits a 
position response and includes a position driving means 
that is responsive to a position command portion of said 
received command signal, and the improvement is fur- 
ther characterized by said adaptive control means fur- 
ther including: 

an adaptive position controller for controlling said 
manipulator’position driving means so that the 
manipulator follows a desired position; and 

said adaptive position controller includes a real-time 
position signal generating means for emitting a 
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signal that causes said manipulator to follow the 
desired position that is indicated by said position 
portion of said command signal either indepen- 
dently of, or simultaneously with, said manipula- 
tor’s force that is being applied to the load or to 5 
said environment. 

18. A hybrid controller in accordance claim 16 

wherein a force control law is executed by said adaptive 
force controller, which force control law is character- 
ized as: 10 

FM = PM + m + W) f ‘ E(t)dt + 

Jo 

Kp(t)E(l) - KM Z (r) 15 

wherein P^t) is said desired force command signal 
term, d(t) is an auxiliary force signal term emitted 
by said adaptation control, and the K /, Kp, and K y 
signal terms are gain terms which are varied by 20 
gain control circuits in said variable control signal 
generating means, E is a force error term which is 
obtained by modifying the desired force command 
signal term by the actual force signal that is sensed 
at the manipulator and Z is a position-related ve- 25 
locity damping term which represents the manipu- 
lator’s velocity in Cartesian space. 

19. A hybrid controller in accordance with claim 18 
and further comprising: 

a linear adaptive position control law represented as 30 
the output signal from said adaptive position con- 
trolling means. 

20. A hybrid controller in accordance with claim 15 
wherein said adaptive control means further comprises: 

an auxiliary signal for controlling, at least in part, the 35 
position and dynamics of said manipulator; and 

summing means for adding said auxiliary signal in 
combination with signals representing position and 
dynamics in said position-controlling adaptive 
feedforward and feedback control loops. 40 

21. A controller for a manipulator wherein the manip- 

ulator and the environment form a complex, dynamic 
model with unknown parameter values, in which there 
exists cross-coupling between position and force vari- 
ables, the improvement characterized by: 45 

position control means for controlling manipulator 
position; 

force control means for controlling manipulator 
force; and 

adaptive control means for compensating for said 50 
cross-coupling which exists between the position 
and force variables applied at said manipulator by 
said position and force control means. 

22. In a hybrid controller adapted to issue a manipula- 
tor control signal which controls a manipulator’s posi- 55 
tion and force in an environment, wherein the manipu- 
lator and environment form a system exhibiting dynam- 
ics which are non-linear and also include manipulator 
and system parameters which are not known to a user 
who controls the manipulator through user-initiated 60 
commands that are issued to the hybrid controller, the 
improvement comprising: 

manipulator driving means responsive to said control 
signal issued by said hybrid controller for driving 
said manipulator in said environment in accordance 65 
with a desired position and a desired force response 
indicated by a command to said hybrid controller, 
which command is issued to said hybrid system 
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free of a priori knowledge of any of said non- 
linearities and/or the manipulator’s and/or sys- 
tem’s parameters; and 

adaptive control means, responsive to said command, 
for generating a variable control signal applied to 
said manipulator’s driving means, which variable 
control signal compensates in real-time for the 
system’s nonlinearities. 

23. A hybrid controller in a system for controlling the 
positioning of, and force exerted by, a controlled manip- 
ulator, said system characterized in that the manipulator 
and the environment form a complex, dynamic model 
specified as having unknown parameter values, and the 
system is further characterized by: 

adaptive control means for issuing a real-time vari- 
able signal to said manipulator that is being con- 
trolled, which signal controls the positioning and 
force of the manipulator, said adaptive controller 
further comprising; 

variable control signal generating means for supply- 
ing said control signal issued by said adaptive con- 
trol means without knowledge of said parameter 
values of the system. 

24. The hybrid controller in accordance with claim 

23, and wherein said manipulator exhibits a force re- 
sponse on a load and includes a driving means that is 
responsive to a force command portion of said control- 
ler’s command signal, and the improvement is further 
characterized by: 

said adaptive control means including at least one 
adaptive force controller for controlling said ma- 
nipulator driving means in the force that it applies 
to the load; and 

said adaptive force controller includes a real-time 
force signal generating means for emitting a force 
control signal that causes said manipulator to ex- 
hibit the desired force response that is indicated by 
said force command portion of said controller’s 
command signal. 

25. The hybrid controller in accordance with claim 

24, and wherein each manipulator additionally exhibits 
a position response and includes a driving means that is 
responsive to a position portion of said command signal, 
and the improvement is further characterized by: 

at least one adaptive position controller for control- 
ling said manipulator’s position driving means in 
order to follow the position response that the ma- 
nipulator is commanded to apply to said load; and 

said adaptive position controller includes a real-time 
position signal generating means for emitting a 
signal that causes said manipulator to exhibit the 
desired position response that is indicated by said 
position portion of said command signal. 

26. A method for controlling a manipulator, wherein 
the manipulator and the environment form a complex, 
dynamic model with specifiable but unknown parame- 
ter values, the improvement characterized by: 

implementing an adaptive position control loop to 
compensate for the specifiable parameter values, 
which control loop operates from the standpont of 
Cartesian space and without knowledge of said 
parameter values based upon a position feedback 
signal which is representative of the manipulator’s 
position in said Cartesian space; 

implementing an adaptive force control loop, which 
control loop operates from the standpoint of Carte- 
sian space and without knowledge of said parame- 



4,860,215 


27 

ter values based upon a force feedback signal 
which is representative of the manipulator’s force 
in said Cartesian space; and 
connecting both control loops to the manipulator to 
be controlled so that the manipulator’s positioning 5 
and it’s force is controlled in Cartesian space by 
both of said two control loops. 

27. In a hybrid controller adapted to issue a control 
signal which moves a manipulator’s position and force 

in an environment, wherein the manipulator and envi- 10 
ronment form a system exhibiting dynamics which are 
non-linear and have specifiable but unknown parame- 
ters, an improved control method comprising: 
controlling said manipulator in said environment in 15 
accordance with a desired position/force response; 
and 

adaptively varying said controlling step by adaptive 
position control and/or adaptive force control; and 
compensating in real-time for the system’s nonlineari- 20 
ties in said adaptively varying control step without 
foreordained knowledge of 

28. A controller for a manipulator wherein the manip- 

ulator and the environment form a complex, dynamic 
model, the improvement characterized by: 25 


28 

adaptive position and adaptive force control means 
controlling a common load in accordance with the 
position and force control laws respectively identi- 
fied by the following algorithms; 

Fyii^m+JpiOEpiO+K^- 
t)E p (t) + C(t)R«)+B0)R(t) +A(,)R(0 

wherein the term R(t) is a desired position trajectory, 
E p (t) is a position tracking error term, and are 
terms contributed by feedback and feedforward 
adaptive position control loops; and 

wherein said force control law is characterized as: 

FM = PM + d(t) + km f ‘ E(t)dt + 

Kp(t)E(t ) - KM Z (t) 

wherein P^t) is said desired force reference signal 
term, d(t) is an auxiliary force signal term emitted 
by said adaptation control, and the K/, K p , and K v 
are variable gain terms; E is an error term; and Z is 
a velocity term. 
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15, 

line 

66 , 

"K 3 " 

should 

read -~K e ~ 




Coluln 

16 , 

line 

9, 

"t. *' 

y 4 

should 

read --y r -- 

• 




Column 16, line 13, "x" should read 



UNITED STATES PATENT AND TRADEMARK OFFICE 

CERTIFICATE OF CORRECTION 

PATENT NO. : 4,860,215 Page 7 of 9 

DATED : August 22, 1989 
INVENTOR(S) :• Homayoun Seraji 

It is certified that error appears in the above-identified patent and that said Letters Patent is hereby 
corrected as shown below: 

Column 16, line 59, "x" (third occurence) should read — x--. 
Column 17, lines 10 and 11, after "B(t) ; " should be --Y r (t)--. 
Column 17, line 10, "e" (second occurence) should read --e--. 
Column 17, line 20, "K" should read — K--. 

Column 17, line 24, "K" should read --K-- and "e" should read 

— “e-- . 

Column 17, line 26, "C" should read --C--. 

Column 17, line 30, "B" should read --B-- and "y" should read 

-y-~. 

Column 17, line 33, "A” should read --A-- and "y" should read 

Y • 

Column 17, line 35, "e" (second occurence) should read --e--. 

Column 19, line 4, "x(t)" (first occurence) should read 
— x ( t) . 




UNITED STATES PATENT AND TRADEMARK OFFICE 

CERTIFICATE OF CORRECTION 

PATENT NO. : 4,86 0,215 Pa ® e 8 of 9 

DATED ; August 22, 1989 
INVENTOR(S) : Homayoun Seraji 

It is certified that error appears in the above-identified patent and that said Letters Patent is hereby 


corrected as shown below: 

Column 23, line 

17, 

" z " 

should 

read --Z--. 

Column 

23, 

line 

56, 

"E" 

should 

read — E-- . 

Column 

23, 

line 

r- 

m 

"E" 

should 

read --E — . 

Column 

23, 

line 

57, 

"R” 

(second occurence) should read 

Column 

23, 

line 

57, 

"R" 

should 

read --R-- . 

Column 

24, 

line 

13, 

h Z " 

should 

read --Z--. 


Column 24, line 65, "manipulator ' position" should read 
--manipulator's position — . 

Column 25, line 15, "Z" should read — Z — . 

Column 27, line 21, after "of" should be —said parameters.--. 



UNITED STATES PATENT AND TRADEMARK OFFICE 

CERTIFICATE OF CORRECTION 

PATENT NO. : 4,860,215 
DATED : August 22 , 1989 

INVENTOR(S) :.Homayoun Seraji 

Page 9 of 9 

It is certified that error appears in the above-identified patent and that said Letters Patent is hereby 
corrected as shown below: 

Column 28, line 9, after "term," should be — fKpEp + K Ep] and 
CCR + BR + hRj — - 

Column 28, line 9, delete "and". 


Column 28, line 17, "Z" should read --Z- 



Signed and Sealed this 
Ninth Day of July, 1991 

Attest: 

HARRY F. MANBECK. JR 

Attesting Officer 

Commissioner of Patents and Tnutcttutrks 




